/**
 * @file /src/main_window.cpp
 *
 * @brief Implementation for the qt gui->
 *
 * @date February 2011
 **/
/*****************************************************************************
** Includes
*****************************************************************************/

#include <QtGui>
#include <QMessageBox>
#include <iostream>
#include <QtWidgets/QAbstractButton>
#include <QListWidget>
#include <QDoubleSpinBox>
#include <QToolBox>
#include <QToolButton>
#include <QAction>
#include <QPushButton>
#include <QMenu>
#include <QSettings>



#define Angle2Pi 0.017453
#define Gait_Num 3
QString gaits[Gait_Num] = {"Stand", "Trot", "Walk"};
QString jump_type[Gait_Num] = {"Jump", "Jump_L", "Jump_H"};
QString au_type[4] = {"STOP", "FRONT", "LEFT", "RIGHT"};

#include "../include/alphadog_monitor/main_window.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace alphadog_monitor {

using namespace Qt;

/*****************************************************************************
** Implementation [MainWindow]
*****************************************************************************/

MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
    : QMainWindow(parent),
      ui(new Ui::MainWindowDesign)
    , qnode(new QNode(argc, argv))

{
    ui->setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class.

    // 生成设置类
    setinf = new Setting;

    // 生成图像绘制类
    graph_plot = new graphplot(this, qnode);

    // 初始化控件
    InitWidget();

    // 读取配置文件
    ReadSettings();

    // 设置初始显示
    ui->MainStack->setCurrentWidget(ui->pageJointsDriver);

    // 页面切换
    connect(ui->Btn_Driver, &QPushButton::clicked, [=]() {
        ui->MainStack->setCurrentWidget(ui->pageJointsDriver);
    });
    connect(ui->Btn_Cmd, &QPushButton::clicked, [=]() {
        ui->MainStack->setCurrentWidget(ui->pageCmd);
    });
    connect(ui->Btn_Image, &QPushButton::clicked, [=]() {
        ui->MainStack->setCurrentWidget(ui->pagePath);
    });
    connect(ui->Btn_Advance, &QPushButton::clicked, [=]() {
        ui->MainStack->setCurrentWidget(ui->pageAdvange);
    });

    // 设置
    connect(ui->Btn_Setting, &QPushButton::clicked, [=]() {
        setinf->Display(0);
    });

    // 设置图像显示
    connect(qnode, SIGNAL(Show_image(int, QImage)), this, SLOT(slot_show_image(int, QImage)));

    // 设置窗口的标题和图案
    setWindowIcon(QIcon(":/images/robot.png"));
    setWindowTitle("Xrobot Monitor");

    // 机器人名称
    QSettings main_setting("alphadog_monitor","settings");
    ui->Lable_RobotName->setText(main_setting.value("robot/name", "robot_name").toString());

    // 定时发布遥控指令
    joystick_timer = new QTimer();
    connect(joystick_timer,SIGNAL(timeout()),this,SLOT(publishJoystickCmd()));
    joystick_timer->start(1);

    // 状态更新
    connect(qnode, SIGNAL(updateState(float*)), this, SLOT(slot_updateState(float*)));
}


MainWindow::~MainWindow() {}

void MainWindow::Display(void){
    this->show();
//    graph_plot->show();
}


void MainWindow::showNoMasterMessage()
{

}

float MainWindow::correctYaw(float yaw){
    int pro_yaw = -yaw * 180. / 3.14 + 180;
    while(pro_yaw < 0){
        pro_yaw += 360;
    }
    while(pro_yaw > 360){
        pro_yaw -= 360;
    }
    return pro_yaw;
}


void MainWindow::slot_updateState(float* data){
    ui->current_x->display(data[0]);
    ui->current_y->display(data[1]);
    ui->current_yaw->setValue(correctYaw(data[2]));

    ui->target_x->display(data[3]);
    ui->target_y->display(data[4]);
    ui->target_yaw->setValue(correctYaw(data[5]));
}

void MainWindow::InitWidget()
{
    // 滑块移动指令
    connect(ui->Slider_LBjoint1,&QSlider::sliderMoved,[=]() {
        ui->Num_LBjoint1->display(ui->Slider_LBjoint1->value());
    });
    connect(ui->Slider_LBjoint2,&QSlider::sliderMoved,[=]() {
        ui->Num_LBjoint2->display(ui->Slider_LBjoint2->value());
    });
    connect(ui->Slider_LBjoint3,&QSlider::sliderMoved,[=]() {
        ui->Num_LBjoint3->display(ui->Slider_LBjoint3->value());
    });

    connect(ui->Slider_RBjoint1,&QSlider::sliderMoved,[=]() {
        ui->Num_RBjoint1->display(ui->Slider_RBjoint1->value());
    });
    connect(ui->Slider_RBjoint2,&QSlider::sliderMoved,[=]() {
        ui->Num_RBjoint2->display(ui->Slider_RBjoint2->value());
    });
    connect(ui->Slider_RBjoint3,&QSlider::sliderMoved,[=]() {
        ui->Num_RBjoint3->display(ui->Slider_RBjoint3->value());
    });

    connect(ui->Slider_LFjoint1,&QSlider::sliderMoved,[=]() {
        ui->Num_LFjoint1->display(ui->Slider_LFjoint1->value());
    });
    connect(ui->Slider_LFjoint2,&QSlider::sliderMoved,[=]() {
        ui->Num_LFjoint2->display(ui->Slider_LFjoint2->value());
    });
    connect(ui->Slider_LFjoint3,&QSlider::sliderMoved,[=]() {
        ui->Num_LFjoint3->display(ui->Slider_LFjoint3->value());
    });

    connect(ui->Slider_RFjoint1,&QSlider::sliderMoved,[=]() {
        ui->Num_RFjoint1->display(ui->Slider_RFjoint1->value());
    });
    connect(ui->Slider_RFjoint2,&QSlider::sliderMoved,[=]() {
        ui->Num_RFjoint2->display(ui->Slider_RFjoint2->value());
    });
    connect(ui->Slider_RFjoint3,&QSlider::sliderMoved,[=]() {
        ui->Num_RFjoint3->display(ui->Slider_RFjoint3->value());
    });

    // 发送单关节控制信息
    connect(ui->Slider_LBjoint1,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_LB1, ui->Slider_LBjoint1->value()*Angle2Pi);
    });
    connect(ui->Slider_LBjoint2,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_LB2, ui->Slider_LBjoint2->value()*Angle2Pi - ang_bias);
    });
    connect(ui->Slider_LBjoint3,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_LB3, ui->Slider_LBjoint3->value()*Angle2Pi + ang_bias);
    });

    connect(ui->Slider_RBjoint1,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_RB1, ui->Slider_RBjoint1->value()*Angle2Pi);
    });
    connect(ui->Slider_RBjoint2,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_RB2, ui->Slider_RBjoint2->value()*Angle2Pi - ang_bias);
    });
    connect(ui->Slider_RBjoint3,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_RB3, ui->Slider_RBjoint3->value()*Angle2Pi + ang_bias);
    });

    connect(ui->Slider_LFjoint1,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_LF1, ui->Slider_LFjoint1->value()*Angle2Pi);
    });
    connect(ui->Slider_LFjoint2,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_LF2, ui->Slider_LFjoint2->value()*Angle2Pi + ang_bias);
    });
    connect(ui->Slider_LFjoint3,&QSlider::sliderReleased,[=]() {
       JointCtrl(JOINT_LF3, ui->Slider_LFjoint3->value()*Angle2Pi - ang_bias);;
    });

    connect(ui->Slider_RFjoint1,&QSlider::sliderReleased,[=]() {
      JointCtrl(JOINT_RF1, ui->Slider_RFjoint1->value()*Angle2Pi);
    });
    connect(ui->Slider_RFjoint2,&QSlider::sliderReleased,[=]() {
      JointCtrl(JOINT_RF2, ui->Slider_RFjoint2->value()*Angle2Pi + ang_bias);
    });
    connect(ui->Slider_RFjoint3,&QSlider::sliderReleased,[=]() {
      JointCtrl(JOINT_RF3, ui->Slider_RFjoint3->value()*Angle2Pi - ang_bias);
    });

    // 足端控制指令
    connect(ui->Btn_LB_FootCmd,&QPushButton::clicked,[=]() {
      FootCtrl(LEG_LB, ui->Box_LBx->value(), ui->Box_LBy->value(), ui->Box_LBz->value());
    });
    connect(ui->Btn_LF_FootCmd,&QPushButton::clicked,[=]() {
      FootCtrl(LEG_LF, ui->Box_LFx->value(), ui->Box_LFy->value(), ui->Box_LFz->value());
    });
    connect(ui->Btn_RB_FootCmd,&QPushButton::clicked,[=]() {
      FootCtrl(LEG_RB, ui->Box_RBx->value(), ui->Box_RBy->value(), ui->Box_RBz->value());
    });
    connect(ui->Btn_RF_FootCmd,&QPushButton::clicked,[=]() {
      FootCtrl(LEG_RF, ui->Box_RFx->value(), ui->Box_RFy->value(), ui->Box_RFz->value());
    });

    // 复位
    connect(ui->Btn_Reset, &QPushButton::clicked,[=]() {
        JointCtrl(JOINT_LB1, 0);JointCtrl(JOINT_LB2, -ang_bias);JointCtrl(JOINT_LB3, ang_bias);
        JointCtrl(JOINT_RB1, 0);JointCtrl(JOINT_RB2, -ang_bias);JointCtrl(JOINT_RB3, ang_bias);
        JointCtrl(JOINT_LF1, 0);JointCtrl(JOINT_LF2, ang_bias);JointCtrl(JOINT_LF3, -ang_bias);
        JointCtrl(JOINT_RF1, 0);JointCtrl(JOINT_RF2, ang_bias);JointCtrl(JOINT_RF3, -ang_bias);
    });

    // 曲线观测器
    connect(ui->Btn_Graph_Plugin,&QPushButton::clicked,[=]() {
      graph_plot->show();
    });

    /* ------------------------------------------------------ 遥控器相关 --------------------------------------------------------- */

    // 启动
    connect(ui->Btn_Enable,&QPushButton::clicked,[=]() {
        if(ui->Btn_Enable->text() == "启动"){
            ctrlEnable = true;
            ui->Btn_Enable->setText("暂停");
            ui->Btn_Enable->setStyleSheet("border:2px groove gray;border-radius:10px;padding:2px 4px;background-color:rgba(255, 0, 0,64);");
        }
        else{
            ctrlEnable = false;
            ui->Btn_Enable->setText("启动");
            ui->Btn_Enable->setStyleSheet("border:2px groove gray;border-radius:10px;padding:2px 4px;background-color:rgba(0, 255, 0,64);");;
        }
    });

    connect(ui->Mode_Enable,&QPushButton::clicked,[=]() {
        if(ui->Mode_Enable->text() == "开启"){
            if(ui->rbtn_nav->isChecked()) mode_ctrl = 1;
            else if(ui->rbtn_route->isChecked()) mode_ctrl = 2;
            else mode_ctrl = 3;
            ui->Mode_Enable->setText("关闭");
            ui->Mode_Enable->setStyleSheet("border:2px groove gray;border-radius:10px;padding:2px 4px;background-color:rgba(255, 0, 0,64);");
        }
        else{
            mode_ctrl = 0;
            ui->Mode_Enable->setText("开启");
            ui->Mode_Enable->setStyleSheet("border:2px groove gray;border-radius:10px;padding:2px 4px;background-color:rgba(0, 255, 0,64);");;
        }
    });

    connect(ui->rbtn_nav,&QPushButton::clicked,[=](){
        if(ui->Mode_Enable->text() == "关闭") mode_ctrl = 1;
    });
    connect(ui->rbtn_route,&QPushButton::clicked,[=](){
        if(ui->Mode_Enable->text() == "关闭") mode_ctrl = 2;
    });
    connect(ui->rbtn_au,&QPushButton::clicked,[=](){
        if(ui->Mode_Enable->text() == "关闭") mode_ctrl = 3;
    });

    // 步态
    SetGaitMenu();

    // 跳跃
    SetJumpMenu();

    // 语音
    SetAUMenu();

    // 路径规划相关
    InitPathWidget();

    // 控制器
    ui->W_arrow->hide();
    ui->A_arrow->hide();
    ui->S_arrow->hide();
    ui->D_arrow->hide();
    ui->Q_arrow->hide();
    ui->E_arrow->hide();

    // 控制器数值初始化
    joystick.mode = 1;
    joystick.p_des[0] = 0;
    joystick.p_des[1] = 0;
    joystick.v_des[0] = 0;
    joystick.v_des[1] = 0;
    joystick.v_des[2] = 0;
    joystick.bodyHeight = -0.29;
    joystick.paceHeight[0] = 0.06;
    joystick.paceHeight[1] = 0.03;
    joystick.gait = 4;
    joystick.jump_triggle = 0;
    joystick.au_type = 0;
}

void MainWindow::InitPathWidget(){
    // connect frame
    cmd_boxes.push_back(ui->box_cmd1);
    cmd_boxes.push_back(ui->box_cmd2);
    cmd_boxes.push_back(ui->box_cmd3);

    for(int i = 0; i < 3; i ++){
        QVector<QLabel*>* vec = new QVector<QLabel*>;
        cmd_data.push_back(*vec);
    }
    cmd_data[0].push_back(ui->cmd1_line1);
    cmd_data[0].push_back(ui->cmd1_line2);
    cmd_data[0].push_back(ui->cmd1_line3);

    cmd_data[1].push_back(ui->cmd2_line1);
    cmd_data[1].push_back(ui->cmd2_line2);
    cmd_data[1].push_back(ui->cmd2_line3);

    cmd_data[2].push_back(ui->cmd3_line1);
    cmd_data[2].push_back(ui->cmd3_line2);
    cmd_data[2].push_back(ui->cmd3_line3);

    // 清空指令
    clearPathCmd();
    ui->path_stack->setCurrentWidget(ui->page_straight);

    // 添加
    connect(ui->btn_AddCmd,&QPushButton::clicked,[=]() {
        addPathCmd();
    });

    // 清空
    connect(ui->btn_ClearCmd,&QPushButton::clicked,[=]() {
        clearPathCmd();
    });

    // 界面切换
    connect(ui->rbtn_straight,&QPushButton::clicked,[=]() {
        ui->path_stack->setCurrentWidget(ui->page_straight);
    });
    connect(ui->rbtn_curve,&QPushButton::clicked,[=]() {
        ui->path_stack->setCurrentWidget(ui->page_curve);
    });

    // 发布
    connect(ui->btn_PublishCmd,&QPushButton::clicked,[=]() {
        route_cmd.position.clear();
        for(int i = 0; i < path_cmd.size(); i ++){
            route_cmd.position.push_back(path_cmd[i].type);
            for(int j = 0; j < 3; j ++) route_cmd.position.push_back(path_cmd[i].cmd[j]);
        }
        qnode->RouteDataPuber.publish(route_cmd);
        clearPathCmd();
    });
}

void MainWindow::clearPathCmd(){
    ui->box_cmd1->setStyleSheet("");
    ui->box_cmd2->setStyleSheet("");
    ui->box_cmd3->setStyleSheet("");

    ui->cmd1_line1->setText(" ");
    ui->cmd1_line2->setText(" ");
    ui->cmd1_line3->setText(" ");

    ui->cmd2_line1->setText(" ");
    ui->cmd2_line2->setText(" ");
    ui->cmd2_line3->setText(" ");

    ui->cmd3_line1->setText(" ");
    ui->cmd3_line2->setText(" ");
    ui->cmd3_line3->setText(" ");

    path_cmd.clear();
}

void MainWindow::addPathCmd(){
    if(path_cmd.size() < 3){
        PathCmd path;
        if(ui->rbtn_straight->isChecked()){
            cmd_boxes[path_cmd.size()]->setStyleSheet("background-color:rgb(233, 185, 110);");
            path.type = 0;
            path.cmd[0] = ui->s_target_x->value();
            path.cmd[1] = ui->s_target_y->value();
            path.cmd[2] = ui->s_target_yaw->value();
            cmd_data[path_cmd.size()][0]->setText("x:"+QString::number(path.cmd[0]));
            cmd_data[path_cmd.size()][1]->setText("y:"+QString::number(path.cmd[1]));
            cmd_data[path_cmd.size()][2]->setText("yaw:"+QString::number(path.cmd[2]));
            path_cmd.push_back(path);
        }
        else{
            cmd_boxes[path_cmd.size()]->setStyleSheet("background-color:rgb(173, 127, 168);");
            path.type = 1;
            path.cmd[0] = ui->c_rotate_radius->value();
            path.cmd[1] = ui->c_rotate_ang->value();
            path.cmd[2] =ui->c_target_yaw->value();
            cmd_data[path_cmd.size()][0]->setText("r:"+QString::number(path.cmd[0]));
            cmd_data[path_cmd.size()][1]->setText("ang:"+QString::number(path.cmd[1]));
            cmd_data[path_cmd.size()][2]->setText("yaw:"+QString::number(path.cmd[2]));
            path_cmd.push_back(path);
        }
    }
    else{
        return;
    }

}
bool MainWindow::connectMaster(QString master_ip, QString ros_ip)
{
  if (!qnode->init(master_ip.toStdString(), ros_ip.toStdString()))
  {
    return false;
  }
  else
  {
      map_rviz = new QRviz(ui->verticalLayout_rviz, "qrviz");
      display_rviz();  
      return true;
  }

}

void MainWindow::on_actionAbout_triggered() {
//    QMessageBox::about(this, tr("About ..."),tr("<h2>PACKAGE_NAME Test Program 0.10</h2><p>Copyright Yujin Robot</p><p>This package needs an about description.</p>"));
}

/*****************************************************************************
** Implementation [Configuration]
*****************************************************************************/

void MainWindow::ReadSettings()
{

}

/**
 * @brief 关闭窗口事件
 * @param event
 */
void MainWindow::closeEvent(QCloseEvent *event)
{

}

/**
 * @brief rviz显示
 */
void MainWindow::display_rviz()
{
    QTimer::singleShot(500,[=](){
        map_rviz->Display_Grid(true, "QGrid", 20,
                               QColor(160, 160, 160));
    });
    map_rviz->Display_RobotModel(true);
}

/**
 * @brief 显示图像
 */
void MainWindow::slot_show_image(int frame_id, QImage image)
{

}


/**
 * @brief 控制单关节
 * @param id 关节号（1-12）
 */
void MainWindow::JointCtrl(JOINT l, float data)
{
    sensor_msgs::JointState msg;
    msg.name.push_back(std::to_string(l));
    msg.position.push_back(data);
    qnode->JointCmdPuber.publish(msg);
}

/**
 * @brief 控制足端
 * @param l
 * @param x
 * @param y
 * @param z
 */
void MainWindow::FootCtrl(LEG l, float x, float y, float z)
{
    sensor_msgs::JointState msg;
    msg.position.push_back(x);
    msg.position.push_back(y);
    msg.position.push_back(z);
    msg.header.seq = l;
    qnode->FootCmdPuber.publish(msg);
}

void MainWindow::SetGaitMenu(void)
{
    QAction* gait[Gait_Num];
    QMenu* menu;

    //创建选择弹出式菜单
    menu = new QMenu(this);
    for(int i = 0; i < Gait_Num; i ++) gait[i] = new QAction;
    for(int i = 0; i < Gait_Num; i ++)
    {
        gait[i]->setText(gaits[i]);
        gait[i]->setCheckable(true);
        if(!i)
        {
            gait[i]->setChecked(true);
            gait_type = gaits[i];
            ui->tBtn_Gait->setText(gait_type);
        }
        menu->addAction(gait[i]);
        connect(gait[i], &QAction::triggered, [=](){
            if(gait[i]->isChecked())
            {
                for(int j = 0; j < Gait_Num; j ++) gait[j]->setChecked(false);
                gait[i]->setChecked(true);
                gait_type = gaits[i];
                ui->tBtn_Gait->setText(gait_type);
            }
        });
    }

    //设置下拉菜单
    ui->tBtn_Gait->setMenu(menu);
}

void MainWindow::SetJumpMenu(void)
{
    QAction* jump[3];
    QMenu* menu;

    //创建选择弹出式菜单
    menu = new QMenu(this);
    for(int i = 0; i < Gait_Num; i ++) jump[i] = new QAction;
    for(int i = 0; i < Gait_Num; i ++)
    {
        jump[i]->setText(jump_type[i]);
        jump[i]->setCheckable(true);
        if(!i)
        {
            jump[i]->setChecked(true);
            _jump = jump_type[i];
            ui->tBtn_Jump->setText(_jump);
        }
        menu->addAction(jump[i]);
        connect(jump[i], &QAction::triggered, [=](){
            if(jump[i]->isChecked())
            {
                for(int j = 0; j < Gait_Num; j ++) jump[j]->setChecked(false);
                jump[i]->setChecked(true);
                _jump = jump_type[i];
                ui->tBtn_Jump->setText(_jump);
            }
        });
    }

    //设置下拉菜单
    ui->tBtn_Jump->setMenu(menu);
}

void MainWindow::SetAUMenu(void){
    QAction* au_cmd[4];
    QMenu* menu;

    //创建选择弹出式菜单
    menu = new QMenu(this);
    for(int i = 0; i < 4; i ++) au_cmd[i] = new QAction;
    for(int i = 0; i < 4; i ++)
    {
        au_cmd[i]->setText(au_type[i]);
        au_cmd[i]->setCheckable(true);
        if(!i)
        {
            au_cmd[i]->setChecked(true);
            _au_cmd = au_type[i];
            ui->tBtn_AUcmd->setText(_au_cmd);
        }
        menu->addAction(au_cmd[i]);
        connect(au_cmd[i], &QAction::triggered, [=](){
            if(au_cmd[i]->isChecked())
            {
                for(int j = 0; j < 4; j ++) au_cmd[j]->setChecked(false);
                au_cmd[i]->setChecked(true);
                _au_cmd = au_type[i];
                ui->tBtn_AUcmd->setText(_au_cmd);
            }
        });
    }

    //设置下拉菜单
    ui->tBtn_AUcmd->setMenu(menu);
}

void MainWindow::keyReleaseEvent(QKeyEvent *event){
    // 判断模式
    if(ui->rBtn_Joystick->isChecked() && ctrlEnable){
        switch(event->key()){
        // 状态控制
        case Qt::Key_Space:
            joystick.jump_triggle = 1;
            break;

        // 速度控制
        case Qt::Key_W: joystick.v_des[0] = 0; ui->W_arrow->hide(); break;
        case Qt::Key_S: joystick.v_des[0] = 0; ui->S_arrow->hide(); break;
        case Qt::Key_A: joystick.v_des[1] = 0; ui->A_arrow->hide(); break;
        case Qt::Key_D: joystick.v_des[1] = 0; ui->D_arrow->hide(); break;
        case Qt::Key_Q: joystick.v_des[2] = 0; ui->Q_arrow->hide(); break;
        case Qt::Key_E: joystick.v_des[2] = 0; ui->E_arrow->hide(); break;

        // 参数控制
        case Qt::Key_G:
            cur_gait = (cur_gait + 1) % Gait_Num;
            ui->tBtn_Gait->menu()->actions()[cur_gait]->trigger();
            break;
        case Qt::Key_H:
            cur_jump = (cur_jump + 1) % 3;
            ui->tBtn_Jump->menu()->actions()[cur_jump]->trigger();
            break;
        case Qt::Key_U:
            ui->pBar_Velocity->setValue(ui->pBar_Velocity->value() + 10);
            break;
        case Qt::Key_J:
            ui->pBar_Velocity->setValue(ui->pBar_Velocity->value() - 10);
            break;
        case Qt::Key_I:
            ui->pBar_Omega->setValue(ui->pBar_Omega->value() + 10);
            break;
        case Qt::Key_K:
            ui->pBar_Omega->setValue(ui->pBar_Omega->value() - 10);
            break;
        }
    }
}

void MainWindow::keyPressEvent(QKeyEvent *event){
    switch(event->key()){
        case Qt::Key_Return:

            joystick.bodyHeight = -(0.2 + ui->Slider_bodyHeight->value() * 0.01);

            // 步态类型
            if(gait_type == QString("Stand")) joystick.gait = 4;
            else if(gait_type == QString("Trot")) joystick.gait = 0;
            else if(gait_type == QString("Walk")) joystick.gait = 3;

            // 跳跃类型
            if(_jump == QString("Jump")) joystick.jump_type = 0;
            else if(_jump == QString("Jump_L")) joystick.jump_type = 1;
            else if(_jump == QString("Jump_H")) joystick.jump_type = 2;

            if(_au_cmd == QString("STOP")) joystick.au_type = 0;
            else if(_au_cmd == QString("FRONT")) joystick.au_type = 1;
            else if(_au_cmd == QString("LEFT")) joystick.au_type = 2;
            else if(_au_cmd == QString("RIGHT")) joystick.au_type = 3;
            break;

        // 速度控制
        case Qt::Key_W: joystick.v_des[0] = ui->pBar_Velocity->value() / 100. * 0.3f;  ui->W_arrow->show(); break;
        case Qt::Key_S: joystick.v_des[0] = -ui->pBar_Velocity->value() / 100. * 0.3f; ui->S_arrow->show(); break;
        case Qt::Key_A: joystick.v_des[1] = ui->pBar_Velocity->value() / 100. * 0.15f; ui->A_arrow->show(); break;
        case Qt::Key_D: joystick.v_des[1] = -ui->pBar_Velocity->value() / 100. * 0.15f; ui->D_arrow->show(); break;
        case Qt::Key_Q: joystick.v_des[2] = ui->pBar_Omega->value() / 100. * 0.3f; ui->Q_arrow->show(); break;
        case Qt::Key_E: joystick.v_des[2] = -ui->pBar_Omega->value() / 100. * 0.3f; ui->E_arrow->show(); break;

    }
}

void MainWindow::publishJoystickCmd(void){
    if(ctrlEnable){
        joystick_cmd.position.clear();
        joystick_cmd.position.push_back(joystick.mode);
        for(int i = 0; i < 2; i ++) joystick_cmd.position.push_back(joystick.p_des[i]);
        for(int i = 0; i < 3; i ++) joystick_cmd.position.push_back(joystick.v_des[i]);
        joystick_cmd.position.push_back(joystick.bodyHeight);
        for(int i = 0; i < 2; i ++) joystick_cmd.position.push_back(joystick.paceHeight[i]);
        if(joystick.v_des[0] || joystick.v_des[1] || joystick.v_des[2]){
            joystick_cmd.position.push_back(joystick.gait);
        }
        else{
            joystick_cmd.position.push_back(4);
        }
        joystick_cmd.position.push_back(joystick.jump_triggle);
        joystick_cmd.position.push_back(joystick.jump_type);
        // 选择控制方式
        joystick_cmd.position.push_back(mode_ctrl);

        joystick_cmd.position.push_back(joystick.au_type);
        qnode->JoystickCmdPuber.publish(joystick_cmd);

        // 清空触发位
        joystick.jump_triggle = 0;
    }
}


}  // namespace alphadog_monitor

